#include "correlate.h"


#if 0
// 关联追踪和检测
void Correlate::process(RadarPoint3D *points, int point_num, Track3D tracks[MAX_TRACKS_3D])
{

    // 雷达检测处理器处理完成，获得目标信息
    m_detected_targets = m_radarProcessor->process(m_tracks); // 检测出来的目标信息

    // 1能否现有行迹关联（最近临关联）能：到卡尔曼滤波进行行迹预测  不能：能否与已有未关联点迹匹配（至少与其他两个点击匹配上）则建立新的行迹
    // 如果不能与未关联点击匹配，则放入为关联点击链表中（此链表每一个点击只保留八帧）

    // 已经存在的行迹进行卡尔曼滤波（用于与下一帧点击进行关联）

    // 如果某行迹（5帧均没有关联上点击），则此行迹删除

    // 输出行迹列表

    //处理一帧三维雷达数据


    //处理一帧数据
    for(int i=0;i<MAX_TRACKS_3D;i++)
    {
        if(m_tracks[i].is_valid)
        {
            kalman3d_predict(&m_tracks)
        }
    }

    // 2. 数据关联与航迹更新
    data_association3d(points, point_num, tracks);

    // 3. 输出有效航迹状态
    printf("=== track result ===\n");
    for (int i = 0; i < MAX_TRACKS_3D; i++)
    {
        if (tracks[i].is_valid)
        {
            printf("track ID: %d\n", tracks[i].id);
            printf("  position: (%.2f, %.2f, %.2f) m\n",
                   tracks[i].kf.x[0], tracks[i].kf.x[1], tracks[i].kf.x[2]);
            printf("  speed: (%.2f, %.2f, %.2f) m/s\n",
                   tracks[i].kf.x[3], tracks[i].kf.x[4], tracks[i].kf.x[5]);
            printf("  life: %d\n", tracks[i].life);
            printf("\n\n");
        }
    }




}
#endif